import cv2
import numpy as np
import time
import threading
import signal
import LeArm
import kinematics as kin
import RPi.GPIO as GPIO

debug = True
ball_color = 'green'

stream = "http://127.0.0.1:8080/?action=stream?dummy=param.mjpg"
cap = cv2.VideoCapture(stream)


# 要识别的颜色字典
color_dist = {'red': {'Lower': np.array([0, 60, 60]), 'Upper': np.array([6, 255, 255])},
              'blue': {'Lower': np.array([100, 80, 46]), 'Upper': np.array([124, 255, 255])},
              'green': {'Lower': np.array([35, 43, 35]), 'Upper': np.array([90, 255, 255])},
              }

# 色块颜色，位置列表
position_color_list = []

# 识别到色块标志
cv_blocks_ok = False
#
x_pix_cm = 0 #色块中心点x坐标
y_pix_cm = 0 #色块中心点y坐标
c_angle = 0  #色块角度


def cv_stop(signum, frame):
    global Running

    print("Stop detection")
    if Running is True:
        Running = False
    cv2.destroyAllWindows()


signal.signal(signal.SIGTSTP, cv_stop)

def leMap(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


while True:
    # if cap.isOpened():
              ret, orgFrame = cap.read()
              if ret:
                  t1 = cv2.getTickCount()
                  try:             
                      orgFrame = cv2.resize(orgFrame, (320,240), interpolation = cv2.INTER_CUBIC) #将图片缩放到 320*240            
                  except Exception as e:
                      print(e)
                      continue
                  if orgFrame is not None:
                    img_h, img_w = orgFrame.shape[:2] #获取图像的高度和宽度
                    # 获取图像中心点坐标x, y
                    img_center_x = img_w / 2
                    img_center_y = img_h / 2
                    if cv_blocks_ok is False:
                        # 高斯模糊
                        gs_frame = cv2.GaussianBlur(orgFrame, (5, 5), 0)
                        # 转换颜色空间
                        hsv = cv2.cvtColor(gs_frame, cv2.COLOR_BGR2HSV)
                        # 查找字典颜色
                        mask = cv2.inRange(hsv, color_dist[ball_color]['Lower'], color_dist[ball_color]['Upper'])
                        # 腐蚀
                        mask = cv2.erode(mask, None, iterations=2)
                        # 膨胀
                        kernel = np.ones((5, 5), np.uint8)
                        mask = cv2.dilate(mask, kernel, iterations=2)
                        # 查找轮廓
                        # cv2.imshow('mask', mask)
                        cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
                        if len(cnts) > 0:
                            # 找出最大的区域
                            c = max(cnts, key=cv2.contourArea)
                            # 返回的值 中心坐标（x, y）,（w，h）,角度
                            rect = cv2.minAreaRect(c)
                            # 获取最小外接矩形的4个顶点
                            box = cv2.boxPoints(rect)
                            # 数据类型转换
                            # 绘制轮廓
                            cv2.drawContours(orgFrame, [np.int0(box)], -1, (0, 255, 255), 2)
                            # 找色块中心点
                            c_x, c_y = rect[0]
                            h, w = rect[1]
                            c_angle = rect[2]
                            if h * w >= 1350:   # 色块面积限制
                                # 绘制中心点
                                cv2.circle(orgFrame, (int(c_x), int(c_y)), 3, (216, 0, 255), -1)
                                x_pix_cm = int(c_x)
                                y_pix_cm = int(c_y)
                                print("x_pix_cm:", x_pix_cm, "y_pix_cm:", y_pix_cm, "c_angle:", c_angle)
                                # cv_blocks_ok = True  # 开始搬运
                    # if cv_blocks_ok and sec_3_flag is False:
                    #     # 数据映射
                    #     n_x = int(leMap(x_pix_cm, 0.0, 320.0, -1250.0, 1250.0)) * 1.0
                    #     n_y = int(leMap(240 - y_pix_cm, 0.0, 240.0, 1250, 3250.0)) * 1.0
                    #     # 需要根据实际情况调整，偏差主要来自舵机的虚位
                    #     if n_x < -100:
                    #         n_x -= 120  # 偏差
                    #     get_d = True
                    #     cv_blocks_ok = False
                    #     if -20 < last_x - n_x < 20:     # 判断色块是否已经不动了
                    #         no_move_count += 1
                    #         if no_move_count >= 4:
                    #             no_move_count = 0
                    #             sec_3_flag = True   # 是就开启定时器
                    #             position_color_list.append((x_pix_cm, y_pix_cm, int(c_angle)))
                    #     else:
                    #         no_move_count = 0
                    #     last_x = n_x
                    # 画图像中心点
                    # cv2.line(orgFrame, (int(img_w / 2) - 20, int(img_h / 2)), (int(img_w / 2) + 20, int(img_h / 2)), (0, 0, 255), 1)
                    # cv2.line(orgFrame, (int(img_w / 2), int(img_h / 2) - 20), (int(img_w / 2), int(img_h / 2) + 20), (0, 0, 255), 1)
                    # t2 = cv2.getTickCount()
                    # time_r = (t2 - t1) / cv2.getTickFrequency()               
                    # fps = 1.0/time_r
                    # if debug:
                    #     cv2.putText(orgFrame, "fps:" + str(int(fps)),
                    #             (10, orgFrame.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, (0, 0, 255), 2)#(0, 0, 255)BGR
                    #     cv2.imshow("orgFrame", orgFrame)
                    #     cv2.waitKey(1)
                    # else:
                    #     time.sleep(0.01)